#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
from vision_messages.srv import MaskPlaneDetect
import message_filters

class Mask_Client():
    def __init__(self):
        self.get_image()

    def client(self):
        rospy.wait_for_service('/perception/mask_with_plane_detection')
        try:
            get_pc = rospy.ServiceProxy('/perception/mask_with_plane_detection', MaskPlaneDetect)
            result = get_pc(self.image, self.depth)
            return result
        except rospy.ServiceException as e:            
            rospy.loginfo("Mask_with_plane_detection Service call failed: %s"%e)

    def get_image(self):
        image_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
        depth_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
        ts = message_filters.TimeSynchronizer([image_sub, depth_sub], 10)
        ts.registerCallback(self.callback)
        
        self.image = rospy.wait_for_message("/camera/color/image_raw", Image)
        self.depth = rospy.wait_for_message("/camera/aligned_depth_to_color/image_raw", Image)


    def callback(self, image, depth):
        # rospy.loginfo('Get Image and Depth!')
        # self.image = image
        # self.depth = depth
        pass

    def image_callback(self, data):
        self.image = data

    def depth_callback(self, data):
        self.depth = data

if __name__ == "__main__":
    pass
